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ABSTRACT 


This thesis presents a method of controlling an autonomous vehicle’s motion in a two 
dimensional environment. Its’ puipose is to expand the functionality of a vehicle’s motion 
by complementing a point to point path planning scheme with a path to path scheme. The 
method introduced in this paper will use the vehicle’s position and the desired path to 
calculate the necessary curvature to effect movement onto the desired reference path. The 
reference path will be a simple planar curve, such as, a circle or line. After successful 
testing of an operating algorithm, the method shall be incorporated into a robot’s software 
system. This path tracking method will lay the groundwork for a dynamic obstacle 
avoidance system for a mobile robot. 
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I. INTRODUCTION 


A. BACKGROUND 

Presently, the bulk of research conducted in the area of path planning for an 
autonomous vehicle deals with sub-dividing the vehicles desired motion into multiple 
independent paths. The most prominent approach has been to describe the desired motion 
tlirough a sequence of configurations. These configurations define the vehicles x, y 
coordinate position coupled with the vehicle’s orientation [Ref. 1J. This method of path 
planning can be best described as a point to point control scheme. As such, a path planning 
algorithm calculates the path between adjacent configurations to find the independent legs 
of the vehicle's motion. This scheme reflects a wide spectrum of motion found within the 
world, and it enjoys many advantages, such as; its’ simplicity, wide applicability, and ease 
of manipulation. However, there are circumstances in which a point to point scheme will 
not fully replicate the desires of the user. 

An example can best be illustrated by reflecting on the motion of a car. Assume a car 
parked alongside the curb desires to pullout into traffic, proceed down the road for two 
blocks, and then make a right turn at the light. In this example there are no predetermined 
points which describe the motion, rather there exist the concept of traffic lanes which define 
and restrict the motion. Although a sequence of points could define a similar range of 
motion, the point to point scheme would require the user to have prior knowledge of 
transition points. In addition, the point to point scheme places no restrictions on the bounds 
of movement between two adjacent configurations. This could raise problems in a dynamic 
environment. On the otherhand, the path tracking method which we shall introduce restricts 
the vehicle's motion to the interior boundaries set by the intersecting paths. Thereby 
increasing control over the vehicle’s motion, while reducing the information needed to 
program a vehicle. In the path tracking method the configurations lose their importance to 
the more general entity of the paths, which are represented as traffic lanes. 
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B. THESIS ORGANIZATION 


The objective of this paper is to develope a mathematical model to support a path to 
path motion control system. The scope of our study will be limited to include simple planar 
curves, such as lines and circles. Upon deriving a mathematical foundation, we shall 
translate the work into a working simulator. This simulator should give the user the 
capability to fully test the algorithm, to include merging a vehicle onto simple planar paths, 
and to transition from path to path. After successful development and testing, the algorithm 
shall be incorporated into the motion control system of an operating vehicle, specifically 
the Yamabico-11 robot. 

The layout of the thesis shall reflect the outline of the research. Chapter II presents a 
detailed problem statement to include our assumptions and requirements for a path tracking 
algorithm. Chapter III will discuss the mathematical groundwork necessary to develope our 
system. In this section we shall derive a control function which will be suitable to merge an 
autonomous vehicles onto a reference path. Chapter IV will then develope the equations 
necessary to support the control function. In the development phase we will separate our 
study into two cases, lines and circles. With the development of an appropriate framework. 
Chapter V will discuss the inherent problems in transitions between paths, and the specifics 
of our transition scheme. Chapter VI will elaborate on the details of implementation of the 
path tracking algorithm into the software of the Yamabico-11 robot. Finally, Chapter VII 
shall be a look at future areas of research and some conclusions. An appendix shall contain 
the path tracking simulator which was written in C. and a users manual for operating the 
path tracking algoritlim. 







n. PROBLEM STATEMENT 


The problem can be stated as follows: find a smooth path from an initial configuration 
p,- to a reference path p re f. The basic concept of our method shall be to vary the vehicles 

instantaneous curvature in order to manipulate the vehicle’s position and heading. To 
facilitate this, we shall expand the concept of a configuration to be a quadruple (x, y, 0, K), 
which describes the vehicles position by it’s cartesian coordinates coupled with it’s heading 
and instantaneous curvature. The reference path will be defined by a similar quadruple. 
This pennits a line or a circle to be represented by the same structure depending on the 
value of k. For example, we might want to solve the problem presented earlier, concerning 
the car entering a traffic lane from a curb position and then making a right turn. The 
vehicles curb position would be represented p, = (0, 0, 0, 0), while the traffic lanes would 
be represented by p ie p = (0. 10. 0.0), and p re f2 = (50, 0, nil . 0) as shown in Figure 1. 








p refI =(0,10,0,0) 
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Figure 1. Vehicle Pulling Away From the Curb 


In developing our solution we shall ignore the size and shape of the vehicle by 
assuming our vehicle is a point robot, and where the clearance to obstacles has been 
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assessed at a higher level within the system. This will simplify our work by removing the 
finite curvature limitation and clearance requirements necessary for a rigid body robot. In 
addition we shall assume the velocity of the vehicle is a constant positive value, and that 
the vehicle's motion will be controlled solely through altering the vehicle’s instantaneous 
curvature. These assumptions will restrict the research to a spatial problem and alleviate the 
need to consider the dynamic relationships inheient to the problem, e.g., time, speed, 
acceleration, and rotation. By making these assumptions we will simplify the mathematical 
model required in finding a suitable path tracking algorithm. 

In our initial problem statement we required that the proposed algorithm produce a 
smooth path. The derivative of curvature is the only control variable within the 
mathematical model. Since the derivative of the curvature dtc/ds is finite, the resultant 
vehicle's trajectory is “smooth" in the sense that the tangent orientation, curvature, and 
derivative of curvature exist at every point on the trajectory {Ref. 2). Although, this concept 
of smoothness is essential, our requirements must be more stringent. The generated path 
must be suitable to be followed by a robot's mechanical power train system. However, 
smoothness is a subjective quality, which must be clearly defined before attempting to 
calculate a viable solution. The problem can be characterized by how rapidly we wish to 
converge onto a desired reference path. If we attempt to converge in too short of a distance, 
the vehicle's motion will be unstable and the vehicle will lose tracking precision. On the 
otherhand. if the distance to converge is too large, then the vehicle’s motion will be 
inhibited by a need for excessive maneuvering space. Therefore, our method will 
incorporate a distance constant. S( } . to regulate the smoothness of the generated paths. The 

distance constant will balance the need for rapid response with the smoothness 
requirement. It's value will be dependant on the maneuvering characteristics of the vehicle, 
and will not adversely effect the mathematical correctness of our work. 
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III. METHOD 


In developing any system the first step is to create a model of the system in order to 
analyze the system. The objective of this is to translate the real world mechanics of the 
system into a mathematical theory which accurately describes the system. This allows one 
to fully explore the nature of the problem and mathematically verify- the solution. As the 
previous chapter outlined our system, this chapter's purpose is to develope the necessary 
mathematical equations to support our previously stated goals and requirements. 

A. PATH CONTROL BY CURVATURE 

When deriving a mathematical equation to simulate a real world problem, it is often 
obvious what type of equation is necessary to solve the problem. Physical models often 
directly translate into applicable mathematical models. However, in our situation we 
undertook this research without a clear idea of the final form of our mathematical model. 
Although we feel it may be possible to solve the problem using traditional control theory- 
methods. we desire an easier method which may prove to yield equally powerful results. 
Therefore, we predicated our solution on the belief that the problem could be solved using 
analysis through differential equations. 

In our problem statement wc introduced the concept of the configuration as a 
quadruple, which defines a vehicle's positional status by stating the vehicle's present 
coordinate position, its’ present direction of motion, and a measure of the change in 
direction of motion. We also defined our desired position by way of a reference path 
defined by the same variables as that of the vehicle's position. This similarity allows us to 
make quick accurate comparisons between the two quadruples. Therefore, we can define a 
function which relates our desired motion with respect to the initial and goal positions. 
Using this idea consider controlling the vehicle by changing its instantaneous curvature as 
determined by a function of the initial and goal quadruples. 





ds =/(K - 9 ’- V - V) 


(3.1) 


Equation (3.1) can be simplified by using the x, and y coordinates to determine, d, the 
distance between the present position and the desired reference path position. However, to 
accomplish this we must narrow the infinite number of available points on the reference 
path down to a specific point of interest. The most logical point would be the point on the 
path closest to the vehicle’s position. We shall define this point to be the image of the 
vehicle’s position, and reference it as p j m ago- Therefore, at all times there will be an image 
point which will provide the positional information of a theoretical vehicle located and 
tracking along the reference path. The establishment of this point allows us to quickly 
calculate the closest distance, d , between the vehicle’s position and the reference path. 
Thereby equation (3.1) becomes 

dK , „ , 

-j- = /(K, 0, d) (3.2) 


Equation (3.2) restates our desire to control the vehicle through changing the 
instantaneous curvature, which is dependent on the vehicular position, t>,, and the reference 
path, p re j The equation does not show the underlying relationship between p,- and p re f, or 
how their values effect the change in curvature. However, since we are attempting to find 
a simple but powerful solution, a logical choice would be to begin with a linear relationship. 
Thus, we propose the following general class as a steering function. 


^ = -[A{ Pi K-p image k) +B{ Pi Q- Pimage 6) +Cd] 


d K 


— +A( Pi K~ Pimag K) +B{ Pi Q- Pimage Q) + Cd = 0 


(3.3) 

(3.4) 


Where A, B, and C are constants. This steering function is a simple linear equation, in which 
the constants can be solved for by using differential equations. 
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B. COEFFICIENTS BY APPROXIMATION 

To find the optimal values for A, B . and C. let us analyze the simplest possible case. 
That is the case in which the reference path, p re f, is the x-axis as shown in Figure 2. 



Figure 2. Vehicle Merging Onto X-axis 

In this case, we can simplify our steering equation due to the following observations. 


Pimage 
P image 


K = 0 

e = o 


d = y 


(3.5) 


That is since the x-axis is the desired reference path, our linear equation can be viewed as 
dependent only on the vehicle’s positional quadruple. Therefore, equation (3.4) becomes 

dK 

— +Ak + BB + Cv = 0 (3.6) 

ds 


Since we know 


y = y (x) 


0 = atan ( v') 



(V') 3 , 00 5 


(3.7) 

(3.8) 
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t 3/2 

(i + (.v'n 


_ d o + oo 2 ) 7 

' rf - v 


(3.10) 


P = y'" (1 + (y') 2 ) 2 - 3v' (v") 2 (1 + (y') 2 ) 3 
By equation (3.6) to (3.11) the steering function becomes, 


nr / // 2 // 

V 3y (y ) ax 

---= - -5 +---: 

2 L t 2 ^ 2 ' 

d + (yV) (i + (y ) ) d + (/)) 


(3.11) 


(3.12) 


_ + M . lv +o , = 0 


However, this equation is too confusing, and as a result very difficult to solve. If we 
are to maintain a simple solution, we must make some assumptions, concerning the terms 
of equation(3.12). To accomplish this we shall make use of two reasonable assumptions. 


00 2 « 1 

y'(y ") 2 « y'" 


(3.13) 


Both assumptions deal with the relative magnitude of the first three derivatives of y. If these 
assumptions hold, which by all indications they do when the value of x is relatively high, 
they would significantly simplify our control equation. Using the assumptions of (3.13), 
equation (3.12) becomes, 

v"' + Ay" + By' + Cy = G (3.14) 


Written in differential notation, we have 


(. D 3 + AD 2 +BD + C)y = C 


(3.15) 


Our steering function thus becomes an ordinary third order differential equation. Since 
the steering function is of the third order, the equation must have at least one real toot. This 
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real loot can be either a positive or a negative value. However, if the real root is a positive 
value the generated solution would diverge from the reference path. Therefore, to generate 
a converging solution, the real root must be a negative value. Let’s assume that the value 
of the root is -A. Then equation (3.15) becomes, 

(-k) 3 + A (-k) 2 + B (~k) + C = C (3.16) 

By solving this equation for the constants, and substituting their value back into the 
equation, we get 

(D 3 +AD 2 + BD + k 3 -Ak 2 + Bk)v = C (3.17) 

(D + k) [. D 2 + (A- k)D + k 3 - Ak 2 + Bk] y = 0 (3.18) 

The second order polynomial of equation (3.18) has two roots. If these roots are 
imaginary, then the solution would be oscillatory and inappropriate for our goal. Therefore, 
we restrict the roots of this polynomial to be negative real roots, which we shall assume to 
be -Aj and -A' 2 - Equation (3.18) now becomes 

(D + k) (D + ki) (D + k 2 )y = 0 (3.19) 

Since, there are no advantages of having three distinct values for k, kj, and k 2 . we shall 
assume the three roots are the same. 

k = A i = A 2 (3.20) 

Therefore, equation (3.19) becomes 

(£> + A) 3 y = 0 (3.21) 

Now. solving for y we get 

v = (^x 1 + Bx + C) E~ kx (3.22) 

Using equations (3.15 and 3.21) we can solve the equation for the values of the 
constants A, B, and C. This is simply accomplished by expanding the third order 
polynomial and individually solving the coefficient for each order. 

D 3 + AD 2 + BD + C = (D + k) 3 (3.23) 
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(3.24) 


D 3 + AD 2 + BD + C s D 3 + 3 kD 2 + 3 k 2 D + k~ 

Therefore, the value of the constants are as follows. 

A = 3k 

B = 3 A- 2 (3.25) 

C = A- 3 

Now that we have calculated the values of the individual constants we can complete 
our steering function by substituting these values into equation (3.3). 

Ys = -^ k <Pi K -Pimagc K ) +3k2 (Pi Q -Pimage e '> + ^ d \ ( 3 - 26 > 

Equation (3.26) is the final fonn of our derived curvature control equation. However, we 
still must determine an appropriate value for the constant k. We know that the coefficients 
of the derived equation will affect the responsiveness and smoothness of the path generated 
by the steering function. Furthermore, we earlier established the distance constant, SQ.for 
the same purpose. Thus a logical solution would be to base the value of k on the value of 
S(,. Therefore, for our algorithm we shall assign the value of k to be the inverse of S 0 . 



(3.27) 


This completes the derivation of our steering function. The output of equation (3.26) should 
generate a smooth path which meets our stated requirements. Preliminary results of the 
steering function are illustrated in Figures 3 and 4. The output displayed in Figure 3 depicts 
the generated paths for various initial configurations and the x-axis being the reference 
path. In each of the cases the initial configuration is the point pj = (0,1) while the vehicle’s 
orientation is the intervals of every 45 degrees. On the otherhand, Figure 4 illustrates the 
effects of changing the value Sq has on the path generated by the algorithm. The output 
illustrated in Figures 3 and 4 are similar to what we generally expected, and fully meet our 
requirements. However, to verify the output and the equation’s suitability, we must check 
it through experimental results. 
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C. EXPERIMENTAL RESULTS 


Ill deriving our curvature control equation we made some assumption vital to 
simplifying our equation. These assumptions dealt with the relative magnitude of the 
derivatives of the v component of the vehicle’s position, and the values of the roots of a 
third order polynomial. Whenever assumptions are made within a mathematical problem 
error is introduced into the final solution. In order to validate the solution and the 
assumptions, we must offer some substantiate proof that the work was not drastically 
flawed by making the assumptions. The best proof is sound mathematical support for the 
assumptions. We can not offer this in our case. However, we can support our assertions via 
experimental results. 

If we can show that the results of our curvature control equation closely match the 
output of a similar system which does not take our assumptions in consideration, then we 
add a degree of validity to our work. Thus, we intend to verify our work by comparing the 
results of our algorithm to the results of an equation derived without our assumptions. Thus, 
we shall compare the output results of our curvature control function of equation (3.26) to 
the assumption free equation(3.22). To accomplish this we shall make the comparison for 
a specific problem. This problem will be the case of merging a vehicle with a initial position 
Pi = (0,1,0,0) onto the directed line, p re f = (0,0,0,0), which is simply the x-axis. By using 
this case we can easily calculate the coefficients for equation (3.22). By solving for the 
variables and using variable substitution we calculated the coefficients' values; 

A - k 2 y 0 

D = ky 0 (3.28) 

C = >0 

Details of the actual mathematical process involved can be found in appendix A. Using 
these values in the assumption free control function we can compare results. 

We conducted four separate cases for comparison using the problem as stated above. 
In each case we solved the problem of merging a vehicle onto the x-axis. while we varied 
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the value of S(> for each test case. The graphical results of both oui curvature control 
algorithm and the actual output as found by the assumption free function is depicted in 
Figures5 to 8. The results of the comparison are very good. In our results we see that the 
output of our path tracking algorithm are very close to the actual values. The test results 
show, that as we increase the distance constant, Sy, the difference in the output becomes 
even less significant, which meets our expectations. Although, the difference increases as 
the distance constant is significantly decreased, as in the case of Sy = 0.125. the output is 
not inappropriate. The results show, that at no instance does an extreme difference in the 
output between the two functions exist. Therefore, the results fully support the assumption 
made within our work. Although, this does not fully validate our results, it does provide a 
significant degree of confidence in our method. With this degree of confidence we have 
faith that we are on the correct track, and that our algorithm is appropriate. 

D. SUMMARY 

In this chapter we laid the mathematical groundwork for our path tracking algoritlun. 
Initially, we had few concrete ideas on the form of our controlling equation. However, our 
guiding factor was to keep the equation simple and powerful. This lead us to guess the 
format of a suitable equation to be a linear differential equation. From this we generated a 
simple steering function based on the difference between the vehicle’s position and the 
vehicle’s image position on a reference path. The resulting differential equation was 
cumbersome and difficult to solve. Therefore, we made some assumption about the relative 
values of the various derivatives of v. This enabled us to reduce the problem to a third order 
differential equation, which could be solved rather easily. Upon solving the differential 
equation. w>e proposed using Sy as the basis for the coefficients of the resulting equation. 

Thereby, allowing us to balance the need for responsiveness with the requirement of 
smoothness. The paths generated by the proposed steering function fully meet our 
expectations. In addition, we tested our results by making a comparison between the output 
of our derived curvature control function and an assumption free version of the steering 
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function. The graphical results showed no significant deviations or aberrant trends between 
the two functions output. Thereby, these experiments fully support the appropriateness of 
our equation, lending a degree of validity to our path tracking method. 
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IV. DETAILED ALGORITHMS 


Chapter III successfully derived a curvature control equation for our path tracking 
algorithm. In this chapter we shall develope the support equations necessary to incorporate 
the control equation into a broader algorithm for path tracking. The overall objective for the 
algorithm is to alter a vehicle’s location defined by a configuration onto a planar curve. To 
accomplish this we shall undertake three steps: to calculate the closest distance between p,- 
and p rc f' to locate p image* and to calculate the change in curvature. With these steps 
accomplished we can construct a program which would successfully maneuver a vehicle 
onto any given reference path. 

A. CLOSEST DISTANCE 

The first step in our general scheme is to calculate the shortest distance between p f - and 
p, c f. We shall refer to this distance as c! c i ose . There are numerous methods to calculate 
distance. However, in our case we desire a method which will be versatile enough to 
calculate this distance given configurations, which may either represent a point, a line, or a 
circle. We do not know of an equation which will allow us to perform such a calculation. 
Therefore, we shall derive a distance equation flexible enough to meet our requirements. 
There are two possible cases in our model; point to line, and point to circle. We shall look 
at these cases separately, and derive suitable equations for each case. 

I. Point to Line 

Our objective is to calculate the shortest distance between any directed line p ref = 
(a, />, a) and a point pj = {x, y). Thus dist{ p re f, p;) refers to the perpendicular distance 
between the point and the line. In preparation for this calculation, let’s calculate the 
distance between the point of interest and a special directed line, po = (0,0, a). This directed 
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line is simply a line parallel to p re f, which originates at the origin. As Figure 9 illustrates, 
using simple trigonometry the distance is 

disripq.pj) = ycos (a) -Asin(a) (4.1) 

Using the results of equation (4.1) we can generate a general distance equation. 
This general equation will be the difference between the distance from the point to the 
refeience path and the distance from the reference path to our special directed line. This 
general equation is illustrated in Figure 10. To calculate the difference we simply apply 
equation (4.1) twice. 

dist(p refPj ) = dist{p 0 ,pj)~dist{p 0 ,p ref ) (4.2) 

= ycos (a) --vsin(a) -b cos (a) +asin(a) (4.3) 

= (y - b) cos (a) - (.v - a) sin (a) (4.4) 

Note, that dist < 0 if the orientation between the directed line and the point is clockwise. 
Thus, this provides us with a suitable equation for the shortest distance between a point and 
any directed line. 

2. Point to Circle 

In deriving the shortest distance from a point pj = (.v,y) to a circle p re f = (*,)',0, k), 
we must realize that two cases exist. The value of K can be either positive or negative. We 
shall examine these two cases separately. Both cases are illustrated in Figure 11 and 12, 
respectively. 

a. Positive Curvature 

Let’s first calculate the origin of the reference circle. This can be done by 
adding the x, and y components of the radius to the x, and y coordinates of the reference 
circle. 
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Figure 9. Distance Between Pj and P 0 
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Figure 11. Positive Curvature Case 



Figure 12 . Negative Curvature Case 
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(4.5) 


fy = P 0 + (rr) c OS(0 o + 7c/2) 
K 0 

>v =Po+ (“) sin (0 O +71/2) 
K o 


Cvy f ) = (a q - ( — ) sin (0.) ,y 0 + (-J-) cos (0,)) (4.6) 

K () K 0 

Now, that we have the coordinates of the origin, we can calculate the distance between the 
origin and the point by using Euler’s distance equation. Furthermore, since we know the 
distance between the origin and the reference path, the radius, we can calculate the distance 
between pj and p re f. This is accomplished by taking the difference between the two 
calculated distances. 


dist(p re fPj) = radius - disr(p orjgjn ,p j ) 


(4.7) 


Kq ai 


sin0. 


a - |a 0 - 


2 ^ 


>’o- 


COS0, 




(4.8) 


We can simplify equation (4.8) by multiplying through with an appropriate factor. 
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sinO, 
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sin 0^1 


O'-.V+nr 


COSOyl 


By factoring we get. 


( 2sin0,.\ / zcosw n \ 

- (A-.V 0 ) -—J- (>’ — >'„) fyj'0-T-J 


0 J 


2cos0„ 


_L 

K () + 4 


(A-.Y 0 ) + 


sin 0^1 


K 


0 J 


COS0 r 


(y - y 0 ) + 


(4.9) 


(4.10) 


Finally, we can multiply by Kq/Kq to simplify equation (4.10). 
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(4.11) 


- (.V — A 0 ) [k 0 (a-.v () ) +2sin0 o ] - (v-y 0 ) Ik 0 (v-.v 0 ) -2cos0 () J 


1 + J[k 0 (a-a () ) +sin0 o ]“+ [tc 0 (y-y 0 ) -cos0 o )~ 

This provides us with the final fonn of our equation. It will calculate the distance between 
any point and any positive curvature circle. Also note that with this equation dist( p re f, p,) < 
0 if the point p, is not circumscribed by the circle. 


b. Negative Curvature 

Figure 10 illustrates the situation when p re f curvature is negative. As this 
situation is veiy similar to the case with a positive curvature, we use the same method to 
calculate the dist( p re f, pj). Howevei. since the curvature of the circle is negative, this will 


give us a negative value for the radius. Thus, in this c^e we subtract the length of the radius 
from the distance between the origin and pj. 


distU> re fPi 


sin© 


A '« 


- v 0 


COS0, 




K„ 


(4.12) 


We can simplify this equation by the method used earlier for the positive curvature case. 
The results of this method will produce the same equation as the positive curvature case. 
However, in this case dist( p re f, pj) < 0 when p, is circumscribed by the circle. 


3. General Distance Equation 

Our objective was to derive a single general equation to calculate the shortest 
distance between a point and a reference path. However, it seems we have derived two 
separate equations to fulfill our needs. Yet, if we take a closer look at the two equations we 
shall reveal an interesting fact. We can consider a directed line, a special case of a circle 
with an infinite radius. The line’s curvature is defined to be equal to zero. Therefore, if we 
implement equation (4.11) in the case of a directed line, we get, 

- (.v — .v () ) fK 0 (.v-.v 0 ) +2sin0 o j - (v-y 0 ) [k 0 (v-v 0 ) -2cos0 o J 
dist= - ; =— =—=====—- = = == = -(4.13) 

1 +,/[k 0 (.v-.v 0 ) +SU 10 () ] 2 + (k () ( y-y 0 ) -cos 0 {) l 2 
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(4.14) 


- (-Y — A' () ) [2sin0 o ] - (v — Vq) [-2cos9 0 ] 

1 + J[sin0 () ] i + [-cos0 o ] 2 

= - (a- - a () ) sin0 o + (v - >- 0 ) cos0 o (4.15) 

This suggests that the distance equation derived in the case that the reference path is a circle 
subsumes both possible reference path cases. Thus, we have established a single general 
equation to determine the shortest distance between any point and any simple planar 
reference curve. 


B. THE IMAGE 

The control equation derived in chapter III was predicated on the establishment of a 
point referred to as Pin ia g e - That is the configuration of a theoretical vehicle, which is 
continuously maintaining position along the reference path. This configuration includes the 
theoretical vehicle’s coordinate position, orientation, and curvature.The difficulty in 
calculating Pi ma ge is that the reference path could be either a line or a circle. Unfortunately, 
there is not a single method, to our knowledge, that would suffice for both of these cases. 
Therefore, we must derive two unique methods to calculate Pin ia g e To accomplish this we 
shall address the two cases separately. 


1. Lines 


The case of finding pj ma g e for a vehicle on a directed line is quite simply. Since 
the curvature of a directed line is defined to be equal to zero, and the orientation of all points 
on a directed line are defined to be equal to the orientation of the directed line itself, the 
calculation of page's curvature and orientation is trivial. That is. 


Pimage* =Prefi 
P image K ~ ® 


(4.16) 


Thus, in the case of the reference path being a line, to calculating Pi mage is reduced to 
calculating the x, and y coordinates of Pimage- Basically, to accomplish this we need to find 
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the shortest distance between the vehicle’s position and the directed line. This situation is 
illustrated in Figure 13. 



Figure 13. Calculating P image for a Line 

To calculate the shortest distance between pj and p re f we use equation (4.11), 
which was developed as a general equation for calculating the closest distance. However, 
we can simplify this equation since the curvature of a line is defined as zero. Therefore, the 
equation for the shortest distance between a point and a directed line as defined with 
configurations is, 

<1 C 1 0SC = ( Pi)' -P ref )') cos (a) - (p-.v - p rcf x) sin (a) (4.17) 

Given the closest distance we can now calculate the coordinate position of Pimage 
using simple trigonometry. Since the distance between the vehicle’s position and the 
reference path is the shortest possible distance, the orientation of the line segment which 
connect the two points must be perpendicular to the orientation of the reference path, as 
shown in Figure 13. The coordinates of Pimage can then he calculated by adding the .v, and 
y components of d c i ose to the vehicle’s coordinate position. Therefore, the coordinates of 

Pimage are * 
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(4.18) 


P„nage X = Pi X + d closS COS (Pi° ~ 7t/2 > > 

Punage? = Pi? + d closed ^, 9 " K/2 ^ 

2. Circles 

Finding p ima g C for the case of p re f being a circle is somewhat trickier than the line 
case, but mathematically the calculations are straightforward. We shall be able to 
distinguish between the two cases by examining p re f K value. If the value is non-zero than 
the desired path is a circle. Otherwise, the path is a line. A circle will be defined by an x, 
and v point which lies on the circle, with this point’s specific orientation and curvature. The 
curvature of a circle is defined to be a constant real number. Therefore, each point on the 
circle will have the same curvature value as p re f curvature. 

Pimage* = Pref* < 4 ' 19 > 

The first step in calculating the other components of the image position’s 
quadruple is to calculate the origin of the reference circle. To do so we calculate the radius 
of the circle, which is defined as the inverse of the curvature. 

radius = —^— (4.20) 

P ref* 

The orientation of a point on a circle is perpendicular to the orientation from the origin to 
the point on the circle, as shown in Figures 11 and 12. Knowing the orientation of the point 
defined as p re f, thus allows us to calculate the orientation from the reference path to the 
origin. Now, by dividing the radius into its’ x, and y components along this orientation we 
will find the coordinates of the origin. The origin shall be referred to as p ori g in . 

Poriein X = Pref X + radius < COS ^refi ~ 7l/2) > 

(4.21) 

Porigin? = Pref? + radius < sin (Prcfi ~ * /2 > > 

Which can be written as, 

Porigin = ( Pref + radius ( sin iP re fi )) 'P re f? + radius (cos (P rej Q) ) ) (4.22) 
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The next step in calculating the coordinate position of Pi magc is to calculate the 
orientation between the origin and the initial position, which we shall refer to as y. This 
orientation can be found using the inverse-tangent function. Thus, y is calculated 


(4.23) 


(Pi- Pori gin- ^ 

y = at an --— (4.23) 

\Pi* Porigin* J 

Unfortunately, this method has two shortcomings. We want to distinguish between pairs of 
points which may have the same inverse-tangent value but different orientations, one such 
pair is -3n/4 and tc/ 4. A second problem is that equation (4.24) is undefined for all pairs of 
points in which pjX = p 0 rigin x - However, we can solve these problems by introducing a 


variation of the normal inverse-tangent function [Ref. 2]. 

y = ^2{ Pj y-p. in y- Pi x-p oriin x) 


(4.24) 


In equation (4.25) we employ an inverse-tangent function of two arguments. This function 
has a range between [jr. -n]. Therefore, it distinguishes between equivalent tangent values 
of different quadrants, and is defined for all values of .v. 

With the orientation between p ; and p or jg in we can now calculate the coordinates 

of Pimage for a circle. The image position is simply the distance of the radius from the origin 
in the direction of y. Therefore, pj B)agc becomes 


Pimagc X = Porigin* + ^ radius ^ cos <Y) 
Pimagey = Porigi + \™dius\ sin (y) 


(4.25) 


The final value we need to calculate for Pimage is the orientation at the image 
point. Once again we shall use the fact that any point on a circle is perpendicular to the 
orientation between the origin and the point. Thus, we can calculate the image's orientation 
by adding or subtracting k/2 to the orientation between the origin and the initial position. 
If the reference path's curvature is negative we subtract, while if it is positive we add. We 
can take advantage of the absolute value function to incorporate both cases into one 
equation. Thus, the orientation of the image point is 
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- T+ («^) (££ 


( 4 . 26 ) 


JWl J 

We have successfully collected all the data needed to find Pi ma ge- With this point 
established we can then go onto creating an algorithm which takes advantage of the 
curvature control equation developed in chapter III. 


C. EVALUATE NEW CONFIGURATION 

We have now calculated all the data we need to assemble a working path tracking 
algorithm. After a user inputs the vehicle’s configuration and the desired reference path's 
configuration, our system would calculate p ima g e , then calculate the closest distance 
between the vehicle and the reference path by our derived d c i osc equation, and finally 
calculate the necessary change in curvature needed to move the vehicle towards and onto 
the reference path. This process would be completed at predetermined intervals, allowing 
foj a means to update the vehicle's positional configuration. However, before we could 
update the vehicle’s configuration we would have to calculate two values, the distance 
traveled by the vehicle each interval, 6 disr, and the change in the vehicle’s orientation each 
interval due to the vehicle’s instantaneous curvature, 68. These values are calculated as 
follows, 

69 = 5 dist (P/K) 

if 69 = 0 then bdist = 6txd 

69 ( 4 - 27 ) 

2 cos — 

if 69*0 then bdist = (5xxv)— 

Where 6t is the duration of the interval, and v is the vehicle’s constant velocity. The 
difference in the calculations for 6 dist is a correction factor which is intended to correct the 
vehicle's coordinate position when it has a non-zero curvature. By calculating these values 
we can update the vehicle’s positional configuration 


30 









(4.28) 



Pj K=p j K + 


dK 

ds 


normipfi + Sdist (p ( tc)) 


PjX = Pj.x + 5dist(cos (pfi + 59)) 
Pj v = PjX + bdisr (sin (/? ( .0 + 50)) 


The algorithm would continuously update the vehicle’s position, and calculate the needed 
change in curvature. This process would effectively smoothly merge the vehicle onto the 
path and maintain it on the path after merging. 

D. SUMMARY 

In this chapter we developed the necessary equation to support an algorithm based on 
the curvature control equation developed in chapter in. This basically consisted of the 
development of the image point on the reference path, and a consistent method to calculate 
distance. With this information we can establish a system which will continuously calculate 
the necessary change in the instantaneous curvature, and update the vehicle's position. This 
will effectively merge the vehicle onto the desired reference path. With this accomplished 
we are ready to implement the algorithm into a vehicle simulator to test the results, and 
verify 1 performance. With satisfactory testing accomplished we could then begin to work on 
a scheme to transition between multiple path. 
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V. TRANSITIONING 


Our path tracking algorithm successfully merged a vehicle onto a reference path, 
therefore the next logical step in developing our path tracking algorithm is to expand the 
algorithm to be able to handle several successive paths. In transitioning between paths we 
must insist that the vehicle’s motion is restricted within the boundary formed by the 
intersecting paths. This requirement is to ensure vehicle safety within an unknown 
environment. Since our algorithm was designed to work for all possible planar lines and 
circles, the problem of executing multiple paths in succession is reduced to the transitioning 
method between successive paths. The primary question to answer is, when do we begin 
our transition from one path onto the next? 

A. POSSIBLE METHODS 

Since we have limited our research to a spatial problem, we have limited the factors 
which effect our transition time. Our problem is not concerned with time, speed, and other 
factors which usually have a bearing on the moment which actions are to occur. Rather, our 
problem is solely dependent on distance. Thus, we have reduced the question of when to 
transition, to at what distance from the next reference path do we begin to transition. There 
are many different options available, however we want to limit the complexity while 
maximizing the effectiveness and fulfilling our safety requirements. Before actually 
detailing our transitioning scheme, let’s look at a few possible options. 

1. Minimum Distance 

The simplest scheme can be devised to transition when the vehicle is within a 
given distance from the next reference path. This distance can be either the shortest distance 
between the vehicle and reference path, or the distance between the vehicle and the 
intersection of the two reference paths. The option chosen is very significant. When the 


vehicle reaches the determined minimum distance, we simply begin using the next 
reference path in the path tracking process. This scheme is simple, and will satisfactorily 
woik in many cases. However, the minimum distance scheme has a serious flaw. 

The minimum distance scheme begins its’ transition the same distance from the 
next reference path for every situation. This scenario works well when the desired turn 
angle approaches 90 degrees. However, problems arise when the interior angle between the 
present reference path and the next reference path deviate greatly from 90 degrees. When 
the turn angle is large the vehicle often does not have sufficient space available to transition 
without crossing through the new reference path. An example of this can be seen in Figure 
14. where a vehicle attempts a 170 degree turn with a minimum transitioning distance of 
three. Although, this distance was sufficient for a 90 degree mm. figure 14 shows that the 
vehicle significantly overshoots the desired reference path. This wastes energy and time, 
and could represent a very serious hazard in an unknown environment. 

Alternatively, when the incident mm angle is small, the moment of transition is 
often earlier than desired. This is due to when we execute the transition, the image is often 
located a significant distance before the intersection point of the two successive paths. In 
many cases this will cause the vehicle to cross the previous reference path during its' 
convergence to the new reference path. In extreme cases the vehicle may even merge onto 
the new reference path prior to the intersection point of the paths. This scenario is depicted 
in figure 1?. where the vehicle attempts a ten degree turn with a minimum transitioning 
distance of three. Once again this deviation from the desired motion wastes energy and 
time, and represents a danger to the vehicle. Therefore, the minimum distance scheme must 
be considered unsuitable for our system. 

2. Dynamic Transitioning 

A second transitioning option would be to vary the transitioning distance 
according to the path tracking problem. The system would dynamically calculate the 
optimum distance to make a smooth efficient tum. To accomplish this method we would 

















establish a turn angle as a reference point on which to vary the transitioning distance. This 
benclunaik would be a 90 degree turn, with a transitioning distance of two times the 
distance constant. Sy. The dynamic scheme would increase this distance for tighter turns, 
and decrease the distance for wider turns. We could further improve this scheme by 
incorporating other factors which affect the vehicle’s maneuverability, e.g. curvature 
limitations, velocity, distance constant, and the vehicle’s maneuverability characteristics. 

The dynamic transitioning described would be extremely effective in optimizing 
the transitions between paths It would maintain the vehicle’s movement to within the 
perimeter outlined by the reference path. This would increase vehicle efficiency, 
maneuverability, and control, which would thereby enhance safety. However, this type of 
system has some disadvantages. The system would require a complex function or a large 
data table to properly determine the correct transitioning distance. Either method would be 
costly in time and hardware support. Furthermore, both options would require exhaustive 
experimentation to derive the necessary data. This would be time consuming and dreary for 
a computer science student. Therefore, we feel although this method yields excellent 
results, it is not appropriate for our system at this time. 

B. A SUITABLE SCHEME 

The transitioning schemes we introduced wore unsatisfactoiy for our system. The 
minimum distance scheme did not fulfill our requirements, while the dynamic scheme was 
too costly. Therefore, the scheme which we settled on was a simplified variation of the 
dynamic transitioning scheme. Our variation will have a transitioning distance function 
based on the turn angle and the distance constant. The transitioning distance will refer to 
the distance from the vehicle to the intersection point of the paths. We will conduct 
transitioning experiments to collect the necessary data concerning the relationship between 
these factors. Given the appropriate data we shall extrapolate a general function which 
meets our requirements. The idea is to derive a function which may not provide the 
optimum transitionmg distance, but will provide an appropriate distance in all cases. 


36 




1. Intersection Point 


Our transitioning distance shall be measured from the vehicle to the intersection 
point of the successive paths. Theiefore, we must have a procedure which locates the 
intersection point for all possible transition. There are four tran' .on possibilities; line to 
line, line to circle, circle to line, and circle to circle. The use r os in our path tracking 
algorithm is for obstacle avoidance. Therefore, it would v>. inappropriate to transition 
between two ciicles. This being the case we shall exclu te the circle to circle case in 
deriving the intersection point for our research. 

a. Line to Line 

The normal line to line intersection problem is very simple when we have the 
lines in the slope intersect format. However, our lines are in the configuration format. The 
method we shall use to calculate the intersection point is based on the Law of Sines. To 
calculate the point of intersection we are going to construct a triangle from the two 
reference paths, calculate the distance of one of the sides of the triangle, and calculate the 
interior angles of the triangle. With this information we can calculate any of the side’s 
distances, and the coordinates of the intersection point. A graphic description of a line 
intersection problem can be seen in Figure 16. We shall now calculate S, the distance 
between the two reference configurations. This can be accomplished using the Euler 
distance equation, 

5 = J(Pre/2 X ~ Prefix'* 2 * ^ ~ P ref^ 

We then calculate the orientation between these two configurations, T, using 
the inverse-tangent function described in equation (4.25). 

T = Xw2(p ier j-p refl y,p ref 2.x-p refl .x) (5.2) 

With this orientation we can construct a triangle from the two directed lines, by projecting 
a line segment in the orientation of T between the two reference paths. We now have the 
necessary information to calculate the interior angles, as shown in Figure 17. 
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P = |/V,e-r] 

DC = |»o/m(7T-r) -p ref: fi\ 
<t> = (71-p-a) 


(5.3) 


With the interior angles calculated we can use the Law of Sines to calculate 
the distance of one of the other two sides of the triangle. We will calculate Sj. the distance 
from the intersection point and the first reference path. 

sin (a) 

This provides us with the needed information to calculate the intersection point. The 
coordinates of the intersection are calculated by dividing Sj into its x, and y components, 
and adding to the coordinates of the reference path configuration 

Pinter x = Prefl X + S l COS (Pref\fy 

Pin,ery = P,ef\y + S l sin (P,eflfy (5-7) 

P inter* = Prefl % -Prefl* 

The orientation of Pin ter is simply equal to the interior angle fonned by the 

intersecting reference paths. This value is being calculated for future use in determining the 
transition distance required. It will be referred to in the future as the turn angle. 


b. Line to Circle/ Circle to Line 

The transition between line to circle and circle to line are very similar. We 
assume the user inputs two path which intersect. Given this assumption, a line will intersect 
with a circle in either one or two points. In either case both scenarios reflect both transition 
possibilities depending on which point of intersection we choose. This example is 
illustrated in Figure 18. The first step in determining the intersection point is to calculate 
the origin of the circular reference path. We calculate the radius, and then use equation (4.6) 
to find the origin’s coordinates. 
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radius = 


1 


Pref K 

Pongin' = Pref' ~ radius < sbl ^,- e / e )) ^ 

Porigh,y = Pre/y + radius ( C0S (Prtf 6 ) > 

The trick now is to create a triangle with p origin and the directed line in order to find the 
intersection point. After creating this triangle we can use the properties of a triangle to 
calculate the measurements of the sides, and to then find the coordinates of the intersection 
point, as shown in Figure 18. To begin let’s calculate the distance from the origin to the 
directed line, A. using equation (4.18). 

(5-9) 


A = Po,ign,y-Prefy > ( cos (/>,,/>)) ~ (Porigin'-Pongin') (P re ft)) 

Using the Pythagorean theorem we can calculate the distance B. If I A I > I radius I, then 
there exists no intersection point between the two paths. 


B = ( —) -A 2 

V Prep 


(5.10) 


We can calculate the image point of the oiigin onto the reference path by 
using the value of A, and the fact that this distance is perpendicular from the origin with 
respect to the directed line. The image is 


Pimage' ~~ Porigin' A (Preft') 

Pimaged ~ Porigin- ~ A C0S ^Pref ^ 

Witli the image calculated we can now find the intersection point. 

Pinter' = Pimage' ±Bcos <Pref» 

Pinteiy ~ Pimaged ~ ^ S ^ rl ^Prcft^ 


(5.11) 


(5.12) 


This equation is suitable for both line to circle and circle to line transitions. If 


we are transferring from a line to a circle we would subtract the components of B from the 
image, and if we go from a circle to line we add the components. To calculate the 






orientation of the intersection point we once again use our variation of the invetse-tangent 
function. 

<I> = atan 2{p inler y-p origi jf inler x-p origin x) (5.13) 

This orientation is perpendicular to the orientation of the intersection point, which can be 
calculated by adding or subtracting n/2 radians, depending on the sign of the curvature of 
the circular reference path. 


p. .0 = <E> + 


inter 


Pref K 
I Pief* 


(5.14) 


Thus, we have successfully calculate the intersection point for all three 
transition possibilities. Using this point we shall be able to calculate the distance from the 
intersection to control the time of transition. Now, we are ready to proceed to detennining 
the transitioning distance function. 


2. Transitioning Distance 

Our transitioning scheme is based on transferring between successive paths at a 
time detennined by the distance from the vehicle to the next path. This distance shall be 
referred to as the transitioning distance. To optimize the efficiency of transitioning the 
transitioning distance can not be a constant value. Therefore, it is our idea to find a function 
that will determine the appropriate distance between the vehicle and path to produce an 
efficient transfer. However, it is highly unlikely that a general function will produce the 
optimum transitioning distance for all possible situations. Thus, our objective is to derive a 
function which will ensure all transfers between paths meet our requirements for safety in 
an efficient manner. The safety requirements are based on maintaining positive control over 
the vehicle to the extent that the vehicle avoid oscillation, and does not cross the bounds 
established by the reference paths. Our method of deriving the function will be based on 
gathering experimental data, which will translate into an appropriate function. 

To derive a transitioning function we intend to conduct experimental tests to 
determine the critical distance necessary for a safe turn. A safe turn is defined to be a turn 


42 



in which the vehicle negotiates the intended turn without crossing the present reference 
path or overshooting the next successive path. The experimental tests will maintain the turn 
angle and the distance constant, while varying the available distance to maneuver the 
vehicle. This process will be repeated until we find the minimum distance needed to fulfill 
the requirements. The range of the experiment will consist of turn angles between [0,7t] at 
intervals of 7t/12. We shall also complete the experiments for various values of Sq. The 
results of our experiments will be put into a table format to enable quick comparison of 
results. We shall then develope a distance function which will approximate the results of 
our experimental data. 

In conducting these experiments the major concern is determining if the generated 
path oscillates or crosses the reference path. To accomplish this we shall manipulate the 
vehicle’s initial position to simulate the desired turn and transitioning distance. To illustrate 
this, we refer to Figure 19. 


Pi =(x,y) 




Figure 19. Rotation of a Transition Problem 
The problems displayed in both figures are identical and will be handled by the simulator 
in the same manner. We are simply changing the vehicle's initial configuration to simulate 
our desired transition. Thus, we convert every transitioning situation into a transition to the 
x-axis. In this case we can quickly determine if the path oscillates or crosses the reference 
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path by simply checking the sign of the vehicle’s position. If the value of the vehicle's v 
component ever goes negative, then the vehicle has crossed the reference path, and the 
transitioning distance is insufficient for that scenario. Thus, it is our desire to translate all 
experimental transitioning into transitions onto the x-axis. 

The graphical results of many of the experiments can be viewed in appendix B. 
Table 1 below shows the experimental data for the simulations. Thus, the table is a concise 
means to display the relationship between the turn angle, distance constant, and 
transitioning distance. The rows represent different distance constants while the columns 
are different turn angle. The entry under any specific row /column is the required minimum 
transitioning distance for that particular problem. For example, the minimum transitioning 
distance for a 90 degree turn with a distance constant of 0.5 is 1.3units, depending on the 
units of Sq. 

3. Deriving A Transitioning Function 

Given this experimental data we can now determine a general function for the 
transitioning distance. Examining the entries of the Table 1. we quickly come to the 
conclusion, that there exists no simple function to determine the transitioning distance for 
all different situations. Therefore, let's attack the problem by dividing the problem into two 
parts, the relationship between the turn angle and the transitioning distance, and the 
relationship between the distance constant and the transitioning distance. By solving these 
two simpler problems and then combining the results, we can derive a composite function 
which is suitable to fulfill our requirements. 

a. Relationship Between TD and Turn Angle 

First let us examine the relationship between the turn angle and the 
transitioning distance. It is an obvious observation that it takes a greater transitioning 
distance to complete a sharper turn than a wider turn. To find the underlying relationship 
between the two values we can plot the entries of the Table 1 for a given value of the 
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distance constant. We can then use this plot to extrapolate a distance function from the 
graph. The example we plotted was the case of Sq = 0.25, and is illustrated in Figure 20. 
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Figure 20. Experimental Minimum Transition Distance 

We plotted the tum angle along the x-axis from -165 to 165 degrees. Since 
the negative tum angles are turns of the same magnitude but in the opposite direction as the 
positive turns, they require the same transitioning distance as their positive counterparts. 
On the y-axis is plotted the minimum required distance to complete a given tum angle. The 
results of our efforts clearly reveal a relationship between these two variables. The shape 
of the graph is similar to that of a parabola, however the base of the curve is much flatter. 
What we desire is to find a function which will map to this plot as close as possible, but 
never produce a transitioning distance smaller than that produced by our experimental data. 
Thus, let's assume the relationship is in the form of a fourth order polynomial, which share 
the basic shape of the parabola. 


7T>(<|>) = 


1 


$ 4 


(5.15) 
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Table 1: EXPERIMENTAL TRANSITIONING DISTANCE 


Turn Angle 
(Degrees) 

Distance Constant 

s 0 =i.o 

Sy=0.5 

Sq=0.25 

S 0 =0.125 

0 

0.0 

0.0 

0.0 

0.0 

15 

2.0 

1.1 

0.6 

0.3 

30 

2.1 

1.1 

0.7 

0.3 

45 

2.1 

1.2 

0.7 

0.4 

60 

2.1 

1.2 

0.8 

0.4 

75 

2.2 

1.2 

0.9 

0.5 

90 

2.3 

1.3 

0.9 

0.5 

105 

2.5 

1.4 

1.0 

0.5 

120 

2.9 

1.6 

1.1 

0.6 

135 

3.5 

1.9 

1.3 

0.7 

150 

4.8 

2.6 

1.7 

1.0 

165 

8.8 

4^ 
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1.9 

180 
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Where. 4> is the turn angle in radians and TD is the transitioning distance. By converting the 
turn angle into a ratio between the turn angle and the maximum possible turn, we simulate 
the curve of Figure 20. Equation (5.15) will produce a flat plot when the turn ratio is close 
to zero, while exponentially increasing the transitioning distance as the tum ratio 
approaches positive or negative unity. 

If we calculate the transitioning distance by this function for those points in 
which we collected experimental data, we can determine the appropriateness of the 
function. We set up a plot similar to that of Figure 20 to display the results of equation 
(5.15). However, we included all tum angles between -170 and 170 degrees. Figure 21 
displays the results. The plot illustrates a very similar curve to that of the experimental data. 
A quick check of the individual values also reveals that the function produces results which 
at all times is greater than those of the experimental data for the case of So equal to 0.25. 
Therefore, this function is an appropriate approximation for determining transitioning 
distance. However, further comparisons between the output of this function and entries in 
Table 1 clearly illustrates that this function does not fulfill our needs. The problem is that 
the function does not take into account the effect altering the value of the distance constant 
has on the minimum transitioning distance. 

b. Relationship Between TD and Sq 

Equation (5.15) is unsuitable for non constant Sq. However, this equation 
does provide us with a base case transitioning function. That is we can use this function to 
determine the transitioning constant, but modify - its’ output as appropriate dependent on the 
actual value of the distance constant. Thus, we are basically creating a composite function 
of a function dependent on the tum angle and a function dependent on the distance constant. 
To derive the function dependent on the distant constant we shall follow the same method 
we used with the tum angle. Therefore, once again let’s plot the experimental results. 
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TRANSITION DISTANCE 



TURN ANGLE (degrees') 

Figure 21. TD as Calculated by Table 1 and Equation (5.15) 
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This would produce 12 independent curves, one for each turn angle, which illustrate the 
relationship between the value of S 0 and TD. The case for turn angles of 150 and 15 degrees 
are displayed in Figure 22. 
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DISTANCE CONSTANT (cm.) 

Figure 22. Relationship Between TD and S 0 

This figures show that the relationship between the value of Sq and TD is linear. In fact, 
each of the twelve plots indicate that this relationship is linear. Therefore, it may be 
possible to extrapolate a linear equation from these plots which will provide a correction 
factor for different distance constants. 

If we begin looking for a relationship between the 12 individual plots, we 
quickly realize that the slope of the twelve plots vary significantly. This would make it very 
difficult to find a linear equation which would satisfy all possible cases. However, since our 
transitioning distance function (5.15) is based on the case of Sq = 0.25, we can standardize 


the entries of Table 1. By plotting the standardized results we can more effectively isolate 
the effect the value of the distance constant has on the transitioning distance. Thus, Table 
2 is an abridged version of Table 1 with the entries converted into ratios. The row/ column 
entries of Table 2 are the ratios between the transitioning distance for a specific tum angle 
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and S 0 , and the standard transitioning distance for that turn with Sq = 0.25. The standard 
transitioning distance is the value outputted by equation (5.15) for a specific turn angle. 


Table 2: TRANSITION DISTANCE RATIO 


Turn Angle 
(Degrees) 

Standard 

TD 

Distance Constant Ratio 

4.0 

2.0 

1.0 

0.5 

15 

1.000 

2.0 

1.1 

0.6 

0.3 

90 

1.067 

2.16 

1.22 

0.84 

0.47 

165 

3.402 

2.56 

1.41 

0.88 

0.56 


Now, if we plot the results of Table 2, we can find an appropriate correction 
factor. Figure 23 depicts the relationship between the ratios of transitioning distance and 
turn angles. 

3 —--- - ----. 



DISTANCE CONSTANT (So C 25) 

Figure 23. Plot of the Ratios of TD and S 0 

In Figure 23 we plotted all three turn angles included in Table 2. As we can see in each case 
the resultant curve is very similar, with nearly the same slope. Thus, this indicates that we 
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can derive a linear correction factor for various distance constants. Using the slope intercept 
line equation we get. 

CF[S 0 } = ( 4.0 " -1.0" )A b {5A6) 

CF(S 0 ) = 0.56.V + b (5.17) 

Where CF is the correction factor. ,y is tire distance constant standardized with respect to 
the value of 0.25. and b is the y-intercept. Using equation (5.17) and the point (1,0.88) from 
our plot we can solve for b. The value of b is 0.32, and equation (5.17) becomes, 

s o 

CF(S 0 ) = (0.56) +0.32 (5.18) 

This equation was a suitable correction factor for various distance constant 
values, however we desire to match the experimental data as close as possible. Therefore, 
we adjusted this linear equation to pioduce results which ' x, ould match the results of the 
transitioning function with those of (he experimental data. We accomplished this simply by 
analyzing the twelve cases and interpolating appropriate values for the slope and intercept. 
The final results were, 

S t) 

CF (Sq) = 0.6(^)+0.3 (5.19) 

By combining the results of both relationships we can create our composite 
function. Thus, our function for determining the transitioning distance for all turn angle and 
distance constants is 


TD{$J {) ) = [2.45 0 + 0.3] 



<t> 

i-G) 

n 


(5.20) 


This equation is simply, but robust enough to apply in all possible transition problems. To 
verify the effectiveness of this equation, we shall compare the results of the equation to the 
experimental results. Table 3 gives us the transitioning distance results by using the derived 






Table 3: CALCULATED TRANSITIONING DISTANCE 


Turn Angle 
(Degrees) 

Distance Constant 

© 

T—< 

II 

» 

00 

S 0 =0.5 

S 0 =0.25 

S 0 =O.125 

0 

0.0 

0.0 

0.0 

0.0 

15 

2.70 

1.50 

0.90 

0.60 

30 

2.702 

1.501 

0.90 

0.60 

45 

2.711 

1.505 

0.904 

0 602 

60 

2.734 

1.519 

0.911 

0.608 

75 

2.784 

1.547 

0.928 

0.619 

90 

2.880 

1.60 

0.960 

0.640 

105 

3.054 

1.696 

1.079 

0.679 

120 

3.365 

1.869 

1.121 

0.748 

135 

3.950 

2.194 

1.317 

0.878 

150 

5.215 

2.897 

1.738 

1.159 

165 

9.186 
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2.041 
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function. As we can see by simple comparison of the entries of Table 1 and Table 3, in all 
cases our derived function outputs a value equal to or greater than that derived by 
experimentation. Therefore, we can assume that our derived equation is appropriate, and 
will ensure sufficient maneuvering space while minimizing waste. 

C. SUMMARY 

In this chapter we introduced some possible transitioning schemes for our algorithm. 
The scheme on which we settled for is a variation of a dynamic scheme. Our scheme is 
based on transitioning a variable distance from the intersection point of two successive 
paths. This distance is determined by a general function which takes into account the degree 
of turn involved and the desired distance constant. The function was derived by gathering 
experimental data through use of our simulator. With the results of these experiments we 
inteipolated a general function, which would fulfill our safety requirements while 
maximizing transitioning efficiency. Some graphical results are included in figures24 to 26, 
which illustrate the effectiveness of our transitioning system. In Figure 24 we are 
completing a simple 90 degree turn with a Sq value of 1.0. Figure 25 combine the line circle 

transitioning combinations, and illustrates the obstacle avoidance problem. In this graph we 
can assume the vehicle isolates an obstacle ahead on its’ present path. The vehicle thus, 
transitions to an appropriate dimension circle to avoid the obstacle. When the vehicle is 
clear of the obstacle it transitions back onto its’ original path. The remaining figure displays 
the transitioning between two circles. 
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Figure 24. Transition From Inner to Outer Circle 











distonce constent-OfcC cm 
(X.Y.Theto.Koppe) (Xj-.Yr.Thetar.Kapper) 
'0 0 -SO. 0 0 0 3) (0 0 -2 0. 0 0. 0 5) 



-2 0 2 

X-AXIS (cm.) 

Figure 25. Transition From Outer to Inner Circle 
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Figure 26. Transitions Involved in Obstacle Avoidance 
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VI. IMPLEMENTATION 


One of the primary objectives of our research was to provide greater flexibility and 
maneuverability for an autonomous vehicle. The path tracking method which we propose 
would accomplish this by providing a simple, but powerful means to control a vehicle’s 
motion. However, to maximize the benefits of our pathtracking algorithm, we should 
incorporate it with a traditional path planning algorithm, such as that described for 
Kanayama's Yamabico-11 [Ref. 3]. The resultant algorithm will enjoy flexibility, 
maneuverability, ease of operation, and increased control. Therefore, our final objective is 
to implement our path tracking algorithm into the software system of an autonomous 
vehicle. Our system has been designed with regards for all categories of vehicles. 
Therefore, our algorithm will be compatible for the Yamabico-11 robot. 

A. OVERVIEW OF SYSTEM 

I. MML System 

The Yamabico-11 robot presently operates within the MML software 
environment. MML is an abbreviation for Model-based Mobile robot Language, and is a 
on-line library of mobile robot oriented functions. MML is a real time motion control 
system for off-line programming of a mobile robot. It consists of three primary 
components; a path planner, a motion generator, and a tracking controller module. The 
system accepts the user’s desires in the form of a user program. The path planning module 
takes this input and converts it into a description of the path. This path description is used 
in conjunction with the velocity and acceleration specifications to produce reference 
configurations which track along the desired path. This data is compared by the tracking 
controller module to minimize the difference between the vehicle’s position and the 
reference configuration. The controller module also consists of all the necessary 
mechanisms for locomotion capabilities including the vehicle's motor and wheels [Ref. 3]. 
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B. PATH TRACKING SYSTEM 


The system we envision shall he comparable to the existing system There shall be two 
modules which parallel the path planner, and the generator. These modules shall be referred 
to as the path descriptor, and the executor. The output of these modules shall provide input 
to the existing tracking controller module. The process shall be initiated by the creation and 
linking of a user module to the path tracking system. Our system shall be written in the C 
language within the Unix environment. 

1. User Program 

The user program shall be designed for off-line programming of the robot. Its 
purpose is to provide a means for the user to program the robot’s motion. The user module 
shall consist of an indeterminate number of path tracking locomotion functions, which 
describe the robot's desired motion. The available commands are described in appendix D. 
The sequence of locomotion commands must be completed by listing the execute command 
after all other commands. Execute acts as a signal to the system to transfer control to the 
executor module. The user program shall be executed under the supervision of the MML 
operating system. The path tracking functions within the user module shall be linked only 
at compile time. This module thereby acts the same as it did for the user program of the 
MML system. 

2. Path Descriptor 

The path descriptor module shall perfonn a similar function to the path planner 
module of the MML system. !* ^hall take a user program and convert it into a description 
of the desired motion in terms acceptable to the executor module. This shall be 
accomplished by taking the sequence of commands of the user program and placing their 
corresponding path information onto an instruction stack. The stack shall comprise of three 
components: the class, the reference path, and the transition point. An example of a typical 
instruction stack is given in Table 4. 
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Table 4: INSTRUCTION STACK 


Class 

Reference Path 

Intersection 

spath 

( x r5- y'r5' ®r5> 1 

— 

ppath 

(x r 4. V r 4. 0 r 4. Kj.4) 

<*i3- ^3- 0 i3< K i3> 

path 

(*r3' 3^3* 

( x i2> yi2. e i2> K i2> 

ppath 

(X r2 . y r2 . e r2 . K r2 ) 

( x ii>yn- e ii. K ii> 

path 

(x r) . y r i - 0 r j, K r) ) 

— 


The class is the type of path locomotion function. There are only three possible 
options: path, ppath. and spath. The path function is an indefinite move command onto the 
path described by the given configuration. The function ppath is a partial path function, 
which is simply a path with a distinct endpoint for transitioning purposes. The path and the 
endpoint are described by the same configuration. The last available class is spath, which 
is a stop path function. The accompanying reference configuration refers to the location on 
the path for which the robot is to stop. 

The reference path and the intersection are configurations which describe the 
desired motion. The user program shall have a path configuration for each command 
entered to delineate a specific path. This configuration shall be placed on the stack as the 
desired reference path. The system shall then use the class and successive reference path 
configurations to determine the intersection point. This intersection point shall be used to 
determine when to transition between successive paths. 

3. Executor 

The executor module will be initiated by the user program. After all locomotion 
functions have been placed onto the instruction stack the command execute will pass 
control to the executor. The objective of the executor is to derive the dK/ds necessaiy to 
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merge onto the reference path. This is simply a call to our path tracking algorithm with the 
specific instruction stack. After the tobot's image has reached the determined transitioning 
point the next entry of the instruction stack shall be executed. This shall be continued until 
all instructions have been executed. At this point the robot shall switch to the stop state. 

C. SUMMARY 

One of the primary objectives of our research was to expand the motion control 
capabilities of all categories of autonomous vehicles. In accordance with this we developed 
a simple but powerful path tracking algorithm. To show the benefits of such an algorithm 
we intend to implement it within the operating system of a mobile robot, Yamabico-11. The 
implementation that we decided to undertake was based on a parallel system to the present 
Yamabico-11 operating system, MML. This would allow us to employ applicable existing 
modules of the MML system, while developing other modules to tailor the system to our 
path tracking algorithm. Our ultimate desire is to create a operating system which could be 
quickly operational, and expanded to subsume the MML system in the future. 
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VII. SUMMARY AND CONCLUSIONS 
A. CONTRIBUTIONS OF RESEARCH 

The benefits of our path tracking algorithm are primarily within two areas. The first 
area is concerned with the effective control of an autonomous vehicle. Most present motion 
control algorithms for autonomous vehicles are based on a exact positioning. That is. the 
vehicle is controlled to pass through certain desired points. This method is extremely useful 
and versatile. However, many instances exist where the exact positioning of die vehicle is 
not as important as the orientation. In our path tracking algorithm we emphasize translating 
our desired motion into simple terms. VVe make it considerably easier for the user to 
program a vehicle’s motion by alleviating the need to predetermine points along the motion 
path. In addition, the general concept of motion behind our path tracking algorithm 
provides a more general approach, which is as encompassing as the more prevalent point 
to point approach. The path tracking approach allows the user to maintain the vehicle on 
curves which would be impossible to replicate in the point to point approach. Both methods 
offer the user some advantages, however it is best when the methods are combined within 
a vehicle’s software. This greatly enhances the vehicle’s motion control flexibility and user 
operability. 

The second area which this research directly benefits is that of obstacle avoidance. In 
our path tracking scheme we included tracking along circular paths. The primary reason we 
implemented a path tracking algorithm for circles is to provide a means for obstacle 
avoidance. Our intentions are, that when a vehicle’s sensors detect an obstacle, we would 
have an automatic transition from the path tlte vehicle is presently on to a circular path 
which would circumvent the obstacle. After avoiding the obstacle the vehicle would then 
transition from the circle back onto the original path. A more complex scheme can be 
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developed on this idea to handle multiple obstacles and dynamic moving obstacles. 
However, the path tracking scheme presents the foundation for these ideas. 

B. FUTURE RESEARCH 

The path tracking scheme developed in this research is a simple algorithm. This simple 
nature provides ample opportunities to improve or expand the system. The possible 
improvements can be conducted mainly in two areas, the mathematical model, and the 
transitioning scheme. 

We developed the algorithm primarily through experimentation. That is we attempted 
to find a valid control method through hunches, and gut instincts. This is not the most 
appropriate means, and it usually does not yield the best results. In deriving our 
mathematical model we included assumptions to ensure a simple control function. In 
addition, the output of the system is not precisely our desired output. A very slight 
oscillation exist in simulations conducted with the value o f Sq less than one. Although the 

magnitude of the oscillation is on the order of 1 O' 6 , it may indicate a problem with out 
method. Therefore, for these reasons it may be beneficial to invest further research into a 
similar mathematical model. This may provide a solid mathematical approach, which 
produces superior results to those presently attained. 

The second area which could be improved upon is the transitioning scheme. We 
developed our transitioning scheme primarily as a result of the limits of oo' resources, both 
time and money. With greater resources available we could investigate whether an 
expanded transitioning scheme would be feasible and economical. If the system merited 
improvement, we could expand the transitioning scheme by taking more factors into 
account m determining the appropriate transitioning distance. The goal would be a fully 
dynamic scheme, which provides the safest, most efficient transitions possible. 

Other research can be accomplished by expanding the present algorithm. The 
algorithm could be expanded by expanding the systems’ applicability, and functionality. 
The applicability of the system could be expanded by increasing the scope of possible 
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reference paths. Thus, we could implement a system in which a robot could path track along 
any given planar curve, including ovals, parabolas, and cubic spirals. This would be 
extremely useful in developing a path tracking scheme based on voronoi boundaries within 
an environment. Another possibility is to expand the algorithm to a three dimensional 
system. This could be useful in motion control of an unteathered submarine, or a mobile 
drone. These options can be accomplished by manipulating the present motion control 
aspects of our system and implementing them in other areas. 

Another research area which could be started in conjunction with our work is obstacle 
avoidance. The path tracking algoritlun lays the groundwork for a possible obstacle 
avoidance algorithm for autonomous vehicles. This is by far the most significant area of 
possible future tesearch. The path tracking algoritlun is simple, and provides a simple 
reliable means to control motion of a vehicle. This is ideal for an obstacle avoidance 
scheme. The gioundwork is established, with further work along these lines an effective 
means to transit unknown environments could be close at hand. 
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APPENDIX A 


To test the correctness of our control equation, we shall compare the results of the 
output of our simulator with that of an assumption free equation based on our system. To 
do this we shall use equation (3.22) and solve for the coefficients A, B, and C. Let’s look 
at equation (3.22) and its’ first and second derivative. 

y = (y-v 2 +B.X + C) £~ kx (A.l) 


y = (Ax + B- y .\ 2 - kBx - kc) C kx 

(A.2) 

n 

y = {A - kA.x- kB - 

- k (Ax + B- - kBx - kC) ) e“ fcv 

(A.3) 

With these equations we can solve for the coefficients. 


y.x = 

o - y 0 c - - v 0 

(A.4) 

/ 

y v = o = tan0 () B = £y 0 +tane o 

(A.5) 

/ 

y 

A - 2kB + A 2 C 


f 2 3/2 

, 3/2 “ K 0 

(A. 6 ) 

d + (y') ) 

(1 + tam 0 o ) 



'x = 0 


A = 2k (tanQy + Ay 0 ) -/: 2 y 0 + K 0 |cose 0 | 3 (A.7) 

To find the specific solution for the coefficients we use the initial position of the robot 
for a given problem. To simplify the problem we shall choose a problem in which the robots 
initial .v coordinate is equal to zero. Thus, we shall solve the coefficients for the problem of 
the robot merging from an initial position Pi = (0, 1, 0, 0) onto the x-axis. Now, we can 
calculate the actual coefficients for this problem. Using equation (A.4) we solve for C. 

C = y 0 = 1 (A. 8 ) 

Now, we solve for B using equations (A.5) and (A. 8 ). 
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B = A- (1) + tan (0) = k 


(A .9) 


And the value of A is calculated. 

A = 2 A (A) -A- 2 (l) + (0) |1| 3 = k 2 (A. 10) 

Since, we have assigned the value of k to be equal to the inverse of Sq, we have all the 
information to plot the desired path free from our earlier assumptions. Thus, we established 
an algorithm which would plot the following function. 

k 2 i 

V = ( y ,v + Aw + 1) e~ kx (A.ll) 

Thus the output of this equation was plotted in conjunction with the output of our simulator 
for various values of Sq. The results of both outputs clearly show the degree of difference 
between our simulator and the assumption free output. 
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APPENDIX B 


In deriving our transition distance function, TD, we conducted numerous simulations 
of transitioning between paths. The objective was to find the minimum distance required to 
complete a specific turn angle. We also conducted these experiments to detennine the effect 
varying the value of So had on the minimum distance. The results where reported in Table 
1. and the graphical results can be found in figure 22 to 32. 

hi conducting the minimum transitioning distance simulation we found a problem with 
the output of our algorithm. The objective of the testing was to find the minimum distance 
to complete the turn without any oscillations or crossing of the reference path. However, as 
we conducted the experiment we found that this requirement was impossible to meet in 
some circumstances. As we decreased the value of Sq we found that a small degree of 

oscillation was present, regardless of how much we increased the transitioning distance. 
This oscillation was present when the value of Sq was 0.5, 0.25, and 0.125. The oscillation 

was a single crossing of the x-axis on the order of 10' 6 . Therefore, we regarded this 
oscillation as insignificant, and proceeded with our testing. However, in the cases where 
the oscillation existed we determined the minimum distance as the point where this slight 
oscillation first appears. 

The output of our testing was four individual paths for each specific turn angle. All of 
the paths were plotted on the same graph for easy comparison. The paths can be 
distinguished from one another since those with the widest and slowest turns have the 
higher values for Sq. Also included on the graph was a dotted directed line, which provides 
the vehicle's initial reference orientation. The ledger found in the lower right-hand comer 
of each graph tell the necessary initial position of the vehicle to complete the turn for a 
given Sq value. 
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APPENDIX C 


/* The header file used in conjunction with the path tracking 
algorithm of pathld.c */ 


/* Constants needed within the program. */ 

♦define PI 3.14159265 

♦define DPI 6.28318530 

♦define RAD 57.2957795 /* 180 / PI */ 

♦define DELTA_TIME 0.001 

♦define MAXSTRING 80 

♦define VELOCITY 10.0 

♦define MAX_DELTA_KAPPA 1.0 

♦define N 10 


/* Structure for reference paths, vehicle's position, image, 
and the intersection point between paths. */ 
typedef struct { 
double x; 

double y; 

double theta; 

double kappa; 

} CONFIG; 


/* Global variables. */ 
typedef struct { 
double xO; 

double y0; 

}POINT; 


/* Procedures defined within the program. */ 
CONFIG current_config, intersect; 

FILE *out_path, *info_file; 

CONFIG path_array[N]; 

POINT intersect; 
int ITERATIONS; 
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double 

CONFIG 

CONFIG 

void 

void 

CONFIG 

void 

void 

CONFIG 

double 

CONFIG 

double 

void 

double 


i_x°, i_yO, dkappa, delta_d, DIST_CONSTANT; 


initial_configuration(); 
initial__reference () ; 
initialize_parameters(); 
initialize_files (); 
intersection_point ()/ 
print_to_file()/ 
update_velocity() ; 
update__configuration () ; 
calc_kappa(); 
calc_image (); 
calc_kappa_dot () / 
time_to_transition() ; 
norm () ; 
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#include 'stdio.h' 
#include "mauh.h" 
#include "path.h" 


main () 

{ 

int path nun = 0; 

int done = 0; 

double dkappa_ds ~ 0.0, speed, delta_dist, aa, bb, cc; 
CONFIG path, image; 

vehicle = initial_configuration(); 
path = initial__ref^rence () ; 
initialize_jparameters (&aa, &bb, &cc) ; 
initialize_files(path); 

intersect = intersection_point(path_num); 
do { 

print_to_file (image path, dkappa_ds); 
update_speed(Sspeed, &delta_dist); 
update_image(&image, vehicle, path); 
delta_d = deIta_distance(vehicle, image); 
dkappa_ds =kappa_dot(vehicle, image, delta_d, delta_dist, 

aa, bb, cc); 

update_configuration(^vehicle, dkappa_ds, delta_dist); 
time_to_transition(Spath, &path_num, &done, image); 

) while (!(done)); 


CONFIG initial_configuration() 

{ 

CONFIG vehicle; 
double dgre; 

printf (''Enter the robots current configuration (X Y DEGREES 
KAPPA) ; " ) ; 

scanf("%lf%lf%lf%lf",&vehicle.x,&vehicle.y,Sdgre, 

&vehicle.kappa'; 







vehicle.theta = norm(dgre / RAD); 
return(vehicle); 


CONFIG initial_reference() 

{ 

double degree, kpa; 
int i, num_of_paths; 

printf("How many reference paths do you desire in the robots 
motion: "); 

scanf ("%d" , &num_of_jpaths) ; 

for (i=0; i< num_of_paths; ++i) { 

printf("\n%s%d%s", "Enter the equation for number ", 

(i+1)," reference path (X Y THETA KAPPA)"); 
scanf("%lf%lf%lf%lf", &path_array[i].x, &path_array[i].y, 
Sdegree, &kpa); 

path_array[i].theta = norm(aegree / RAD); 
path_array[i].kappa = kpa; 

} 

path_array(num_of_paths].x = 0.0; 
path_array[num_of_paths].y = 0.0; 
path_array [num_of_jpaths] . theta = 6.28; 
path_array [num_of_jpaths] . kappa = 0.0; 
return(path_array[0] ) ; 


void initialize_parameters(aa, bb, cc) 
double *aa, *bb, *cc; 

{ 

double kk; 

printf("What is the desired value of the distance 
constant: "); 

scanf("%lf", &DIST_CONSTANT); 
kk = 1.0 / DIS T_CONS TANT; 

*aa = 3.0 * kk; 
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*bb = *aa * kk; 

*cc = *bb * kk / 3.0; 


void initialize_files(path) 

CONFIG path; 

{ 

char name[MAXSTRING], namel[MAXSTRING]; 

printf("What is the name of the graph output file? "); 
scanf ("%s", name); 

printf(”What is the name of the info output file? ”); 

scanf {"%s", namel); 

info_file = fopen(namel, "w"); 

out_path = fopen(name, ”w"); 

fprintf(out_path, "%s%.If\n%s\n", "#cs ", 0.5, "#lg 2"); 
fprintf (out_path, "%s%.2f%s\n", 

"#leg \"distance constant=", DIST_CONSTANT, "\"") 
fprintf(out_path, "%s%27s\n", "#leg \"(X,Y,Theta,Kappa)", 
" (Xr,Yr,Thetar,Kappar)\""); fprintf(out_path, 
"%s%.lf, %.lf, %.lf, %.lf%s%5s%.lf, %.lf, %.lf, 
%.lf%s\n", "#leg \"(", vehicle.x, vehicle.y, 
vehicle.theta*RAD, vehicle.kappa,")","(", 
path.x,path.y,path.theta * RAD, path.kappa,")\"") 


void print_to_file (image, path, dkappa_ds) 
CONFIG image, path; 
double dk app a_ds; 

{ 


printf ( "%lf ", vehicle.x); 

printf( " %lf\n ", vehicle.y); 

fprintf(out_path, "%lf ", vehicle.x); 

fprintf (out_path, " %lf\n ", vehicle.y); 

fprintf(info_file, "%s%lf ", "x=", vehicle.x); 

fprintf(info_file, "%s%lf ", "y=", vehicle.y); 

fprjntf(info_file, "%s%lf ", "theta=", vehicle.theta); 
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} 


fprintf(info_file, 
fprintf(info_file, 
fprintf(info_file, 
fprintf(info_file, 
fprintf(info_file, 
fprintf (info__file, 
fprintf(info_file, 
fprintf(info_file, 
fprintf(info_file, 
fprintf(info_file, 
fprintf(info_file, 
fprintf(info file, 


"%s%lf\n ", "kappa=", vehicle.kappa); 
"%s?.lf ","itheta=", image.theta); 
"%s%.5f ", "image_x=", i_xO); 

"%s%.5f ", "image_y=", i_yO); 

"%s%lf\n ", "close_dist=", delta_d); 
"%s%lf ","dkappa_ds=", dkappa_ds)/ 
"%s%lf ","inter_x=", intersect.x); 
"%s%lf\n ","inter_y=", intersect.y); 
"%s%lf ", "path.x=", path.x); 

"%s%lf ","path.y=", path.y); 

"%s%lf ","path.t=", path.theta)/ 
"%s%lf\n\n ","path.k=", path.kappa); 


void update_speed(speed, delta_dist) 
double *speed, *delta_dist; 

{ 

*speed = VELOCITY; 

*delta_dist = DELTA_TIME * *speed; 


double delta_distance(vehicle, path) 

CONFIG vehicle, path; 

{ 

double distance; 

distance = (-(vehicle.x - path.x) * (path.kappa * 

(vehicle.x - path.x) + 2 * sin (path.theta)) - 
(vehicle.y - path.y) * (path.kappa * 

(vehicle.y - path.y) - 2 * cos(path.theta))) / 
(1 + sqrt (pow (path.kappa * (vehicle.- path.x) 

+ sin (path.theta), 2.0) + pow (path.kappa * 
(vehicle.y - path.y) - cos (path.theta), 2.0))); 

return(distance); 

} 
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void update_image(image, vehicle, path) 

CONFIG *image, vehicle, path; 

{ 

double radius, gamma, close_dist; 

POINT origin; 

if (path.kappa ==0.0) { 

close_dist = (((vehicle.y - path.y) * cos(path.theta) ) 
( (vehicle.x - path.x) * sin(path.theta) )) ; 
(*image).x = vehicle.x + close_dist * sin(path.theta) 
(*image).y = vehicle.y - close_dist * cos(path.theta) 
(*image).theta = path.theta; 

(*image).kappa = path.kappa; 

) 

else { 

radius = (1.0 / path.kappa); 

origin.xO = path.x - radius * (sin (path.theta)); 
origin.yO = path.y + radius * (cos(path.theta)); 
gamma = atan2(vehicle.y - origin.yO, 
vehicle.x - origin.xO); 

(*image).x = origin.xO + fabs(radius) * (cos(gamma)); 
(*image).y = origin.yO + fabs(radius) * (sin(gamma)); 
<*image).theta = norm(gamma + (PI/2)* (path.kappa/ 
fabs (path.kappa))); 

(*image).kappa = path.kappa; 

} 

i_x0 = (*image).x; 
i_y0 = (*image).y; 


double kappa_dot(vehicle, image, delta_d, delta_dist, 
aa, bb, cc) 

CONFIG vehicle, image; 

double delta_dist, delta_d, aa, bb, cc; 

{ 

double delta_kappa, dkappal; 
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dkappal = -aa ♦ (vehicle•kappa - image.kappa) 

-bb * (norm(vehicle.theta - image.theta)) 
-cc * delta_d; 

delta_kappa = dkappal * delta_dist; 
return(delta_kappa); 

} 


void update_configuration(vehicle, dkappa_ds, delta_dist) 

CONFIG *vehicle; 

double dkappa_ds, delta_dist; 

{ 

double delta_theta, delta_distl, kappa; 
double epsilon = 0.00001; 

kappa = (^vehicle).kappa + dkappa_ds; 
delta_theta = delta_dist * kappa; 
delta_distl = delta_dist; 
if (fabs(delta_theta) <= epsilon) { 

delta_distl =delta_dist ♦ (sin (delta_theta/2) / 
(delta_theta/2)); 

} 

(♦vehicle).x += (cos((^vehicle).theta + delta_theta / 2.0) 

♦ delta_distl); 

(♦vehicle).y += (sin((♦vehicle).theta + delta_theta / 2.0) 

♦ delta_distl); 

(♦vehicle).theta = norm((*vehicle).theta + delta_theta); 
(♦vehicle).kappa = kappa; 


void time_to_transition(path, path num, done, image) 

CONFIG *path, image; 
int *path_num, *done; 

{ 

double distance, turn_angle, TDist; 

if (path_array [ *path__num+l ].theta == 6.28){ 

if (((fabs(delta_d) < 0.00001) && (fabs((*path).kappa - 
vehicle.kappa) <= 0.00001) && (fabs(norm (vehicle.theta - 


85 





image.theta)) <= 0.00001)) || (ITERATIONS > 2000)) 

*done = 1; 
else { 

♦done = 0; 

ITERATIONS += 1; 

) 

} 

else { 

turn_angle = intersect.theta; 

distance = sqrt(pow(image.x - intersect.x, 2.0) + 
pow(image.y - intersect.y, 2.0)); 

TDist = (2.4 * DIST_CONSTANT + 0.3) * (1/(1- pow (turn_angle 
/ PI, 4.0))); 
if (distance < TDist ) { 

♦path = path_array[♦path_num + 1 ] ; 

*path_num += 1; 

intersect = intersection_jpoint (*path_num) ; 

♦done = 0; 

fprintf (out_jpath, "%s% . If, %.lf, %.lf, %. lf%s%5s%. If, %.lf, 
%.lf, %. If %s\n", "#leg \”(", vehicle.x, vehicle.y, 
vehicle.theta + RAD, vehicle.kappa,")”,"(”, 

(♦path).x,(*path).y, (*path).theta * RAD, 

(♦path).kapra,")\""); 

) 

else 

♦done = 0; 

} 

} 


CONFIG intersection_point(path_num) 
int path__num; 

{ 

double dist_refs, ref_orient, beta, alpha, sigma, inter_d, 
distance, distancel, phi, intersect_orient; 

POINT imagel, origin; 

CONFIG inter; 

if (path_array[path_num+l].theta == 6.28) { 

inter.x=path_array[path_num].x + 

1000*cos(path_array(path_num].theta); 
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inter . y=pat!i sn;~y [oath Rum] .y + 

100G*sin (p^b array[path_num] .theta) ; 

I 

else if ( (path_array [p? : *•; numj. kappa == 0.0) && 

(path_array[paLh_oum+l].kappa == 0.0))] 
dist__refs=sqrt (pow (path_array [path_num +l].x - 
path_array[path_num].x, 2.0) + 
pew(path_array[path_num +1].y - 
path_array[path_numj.y, 2.0)); 
ref_orient = atan2(path_array[path_num +l].y - 
path_array[path_num].y, 
path_array[path_num +1].x - 
path__array [path_num] . x) ; 
beta = path_arrayfpath_num].theta - ref__orient; 
alpha = norm(PI - ref_orient) - path_array [path__num + 

1].theta; 

sigma = norm(PI - beta - alpha); 

inter_d = dist_refs * (sin (alpha)/sin(sigma)); 

inter.x=path_array[path_num].x + 

inter_d*cos(path_array[path_num].theta); 
inter.y=path_array[path_num].y + 

inter_d*sin(path_array[path_num].theta); 
inter.theta = norm(path_array[path_num + 1].theta - 
path_array[path_num].theta); 

} 

else if(path_array[path_num].kappa == 0.0) { 

origin.xO = path_array[path_num+l].x - (1/ 
path_array[path_num+l].kappa) * 

(sin(path_array[path_num+l].theta)); 
origin.yO = path_array[path_num+l].y + (1/ 
path_array[path_num+l].kappa) * 

(cos(path_array[path_num+l].theta)); 
distance = (origin.yO - path_array[path_num].y) * 

cos<path_array[path_num].theta) - (origin.xO - 
path_array[path_num].x) * 
sin(path_array[path_num].theta); 
distancel =sqrt (pow (l/'path_array [path_num+l ] . kappa, 2.0)- 
pow(distance,2.0)); 
imagel.xO = origin.xO + distance * 

sin (path_array [path_num] .theta) .* 
imagel.yO = origin.yO - distance * 

cos(path_array[path_num].theta); 
inter.x= imagel.xO - distancel 
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*cos(path_array[path_num].theta); 
inter.y= imagel.yO - distancel 

*sin(path_array[path_num].theta); 
phi = atan2(inter.y - origin.yO, inter.x - origin.xO); 
intersect_orient = norm(phi + (PI/2) * 

(path_array[path_num +1].kappa / 
fabs(path_array[path_num + 1].kappa))); 
inter.theta = norm(intersect_orient - 
path_array[path_num].theta); 

) 

else { 

origin.xO = path_array[path_num].x - (1/ 
path_array[path_num].kappa) * 

(sin(path_array[path_num].theta)); 
origin.yO = path_array[path_num].y + (1/ 
path_array[path_num].kappa) * 

(cos(path_array[path_num].theta)); 
distance = (origin.yO - path_array[path_num+l].y) * 

cos(path_array[path_num+l].theta) - (origin.xO - 
path_array[path_num+l).x) * 
sin(path_array[path_num+l).theta); 
distancel = sqrt(pow(l/path_array[path_num].kappa,2.0)- 
pow (distance. 2.0)); 
imagel.xO = origin.xO + distance * 

sin(path_array[path_num+l].theta); 
imagel.yO = origin.yO - distance * 

cos(path_array[path_num+l].theta); 
inter.x= imagel.xO + distancel 

*cos (path_array [path_num+l] .theta) ; 
inter.y= imagel.yO + distancel 

*sin(path_array[path_num+l].theta); 
phi = atan2(inter.y - origin.yO, inter.x - origin.xO); 
intersect_orient = norm(phi + (PI/2) * 

(path_array[path_num].kappa / 
fabs(path_array[path_num].kappa))); 
inter.theta = norm(- intersect_orient + 
path_array[path_num + 1].theta); 

} 

return(inter); 

} 


ss 


double norm(angle) 
double angle; 

/ 


while ((angle > PI) || (angle 
if (angle > PI) 
angle -= DPI; 
else 

angle += DPI; 

} 

return (angle); 


} 



APPENDIX D 


We propose a path tracking algorithm which is applicable to any autonomous vehicle. 
This algorithm would be operable in conjunction with the present motion control software 
system MML for Yamabico. An overview of the present locomotion functions is presented 
by Kanayama [Ref. 4], which seconds as Yamabico’s user manual. This reference gives the 
reader a clear understanding of the MML software environment, and the available motion 
control commands. The modification which would be necessary to combine the two 
independent systems was explained in chapter VII. In this appendix we would like to 
explain the additions necessary to the library functions in the MML language by expanding 
the user's manual. 

Tire changes which are necessary are concerned exclusively with the sequential 
locomotion functions, such as move and stop. To append the users manual we must simply 
add three locomotion functions to the system. These three commands shall allow the user 
to intersperse the functional features of the path tracking algorithm with the established 
point to point motion control scheme. The added commands will be labeled as follows; 
path,ppath, and spath. It is our hope, that by incorporating both systems together with as 
few as possible additional commands, we can maintain the entire MML environment. In 
addition, we feel this will greatly expand the robot's functionality and versatility without 
complicating the operating procedure of the robot. Let’s take a in-depth look at each new 
sequential locomotion function. 

PATH TRACKING LOCOMOTION FUNCTIONS: 

MOVE PATH 

SYNOPSIS: patl ?(c); 

CONFIGURATION *c; 
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DESCRIPTION: 


This function moves the robot from the robot’s current 
configuration p; = (.vj, _vj, 9j, Kj) onto the directed 
reference path = (.v, v, 0, k). The present speed and 
acceleration, v c and a c , are used for the motion. The 
robot’s motion should smoothly merge onto the 
reference path and continue to track along the path 
indefinitely or until another motion function is 
encountered. The only acceptable command following a 
path() is another path tracking locomotion function, 
or an immediate function. The locomotion function 
which follows this command must consist of a path 
which intersects the present desired path. If the two 
consecutive paths do not intersect, the robot stays on the 
current reference path. 

ERROR: If the command following path() is not an acceptable 

option, the robot stops and an error code is returned. 

SEE ALSO: ppath(), spath(), speed(), acc(), movei(). 

MOVE PARTIAL PATH 

SYNOPSIS: ppath{ c); 

CONFIGURATION *c; 

DESCRIPTION: This function is a variation of the path function. It moves 

the robot from the robot’s 

cunrent configuration p; = (,\j, Vj, 0j, Kj) onto the 
desired reference path. The present speed and 
acceleration, v c and a c , are used for the motion. The 
robot’s should smoothly merge onto the desired 
reference path and continue to track along the path until 
its image reaches c = (,v, v, 0, k). At this point control 
goes to the next path function. 

If the destination point c is the same as the current 
nominal point, the robot stops and an error code is 
returned. 

path(), spathQ, speed(), acc(), movei(), move(), 
stop(). 


ERROR: 


SEE ALSO: 
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STOP PATH 


SYNOPSIS: 

DESCRIPTION: 

SEE ALSO: 

SPECIFYING S 0 
SYNOPSIS: 

DESCRIPTION: 

ERROR: 


spathic ); 

CONFIGURATION *c; 

This function is a variation of the path() function. It 
moves the robot from the robot’s current configuration 
Pi = ( A ‘i> }'i> ©i> K j) onto the desired reference path. The 
present speed and acceleration, v c and a c , are used for 
the motion. The robot should smoothly merge onto the 
desired reference path and continue to track along the 
path until its image stops at c = (.v, y, 9, k). At this point 
the robot switches to the STOP state. In this state the 
robot can complete any command of either system. 

path(). ppath(). speed(), acc(), movei(), move(). 
stop(). 


dist_const(x)\ 
double x; 

This function allows the user to adjust the value of So 
for a particular maneuver. The command can only be 
used while the robot is in the STOP state. 

If the user inputs a negative value for the distance 
constant, the robot stops and an error code is returned. 
If this command is given while the robot is not in the 
STOP state, the robot shall stop and an error code 
returned. 


92 






LIST OF REFERENCES 


Kanayama, Y., and Hartman B.I., "Smooth Local Path 
Planning for Autonomous Vehicles, Part I: 
Symmetetricity," Proceedings IEEE Journal of Robotics 
and Automation, pp. 1265-1270, 3989. 

Kanayama, Y., "Introduction to Two Dimensional Spatial 
Reasoning," paper presented at the Naval Postgraduate 
School, Monterey, CA., 24 March 1991. 

Technical Report of Naval Postgraduate School, 

"Locomotion Functions in the Mobile Robot Language, 
MML," Kanayama, Y., and Onishi, M., pp.l - 23, 1990. 


93 







BIBLIOGRAPHY 


1. Akman, V., Unobstructed Shortest Paths in 
Polyhedral Environments, Springer-Verlag, 1987. 

2. Brady, M. , Robot Motion: Planning and Control, MIT 
Press, 1982. 

3. Canny, J., The Complexity of Robot Motion Planning, 
MIT Press, 1988. 

4. Gordpasture, R. P., A Computer Simulation Study of 
an Expert System for Walking Machine Motion 
Planning, National Technical Information Service, 

1987. 

5. Kanayama, Y., and Hartman B.I., "Smooth Local Path 
Planning for Autonomous Vehicles, Part II: 

Cubic Spirals," Proceedings IEEE Journal of Robotics 
and Automation, 1989. 

6. Kanayama, Y., and Noguchi, T., "Locomotion Functions 
for a Mobile Robot Language ," Proceedings IEEE/RSJ 
International Workshop on Robot Programming 
Languages, 1989. 

7. Kanayama, Y., and Onishi, M., "Locomotion Functions 
in the Mobile Robot Language, MML," Proceedings IEEE 
Journal of Robotics and Automation, pp. 1110 -1115, 
1991. 

8. Kanayama, Y., and Yuta, S., "Vehicle Path 
Specification by a Sequence of Straight Lines," IEEE 
Journal of Robotics and Automation, vol. 4, no. 3, 

1988. 

9- Nelson, W., and Cox, I., "Local Path Control for an 

Autonomous Vehicle," Proceedings IEEE Conference on 
Robotics and Automation, 1988. 


94 



Richbourg, R. F., Solving a Class of Spatial 
Reasoning Problems: Minimal-Cost Path Planning in the 
Cartesian Plane, National Technical Information 
Service, 1987. 

Sanders, D. W., A feasibility Study in Path Planning 
Applications Using Optimization Techniques, National 
Technical Information Service, 1987. 

Schwartz, J. T., Sharir, M., and Hopcroft, J. , 
Planning Geometry, and Complexity of Robot Motion, 
Abler Pub, 1987. 

Smith, W., Local Path Planning using Optimal 
Control Techniques, National Technical Information 
Service, 1988. 







INITIAL DISTRIBUTION LIST 


4 


Defense Technical Information Center 2 

Cameron Station 
Alexandria. VA 22304-6145 

Dudley Knox Library 2 

Code 52 

Naval Postgraduate School 
Monterey, CA 93943 

Chairman. Code CS 2 

Computer Science Department 
Naval Postgraduate -School 
Monterey, CA 93943 

Dr. Yutaka Kanayama, Code CS/Ka 2 

Computer Science Department 
Naval Postgraduate School 
Monterey. CA 93943 

LT Richard J. Abresch 2 

1640 Amberlea Dr. 

Dunedin, FL 33528 


96 








